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ABSTRACT 



Here is a simple example of control of inherently unstable 
system. An inverted pendulum pivoted on top of a cart is to be 
stabilized by applying force to the cart through an electric motor. 

In the electrical laboratory of the United States Naval Post- 
graduate School, a cart with a stick pivoted on top of it has been 
built, tested and simulated with CDC 1604 digital computer. 

The author. Lieutenant Mu-yu Wan of the Chinese Navy, wishes 
to thank Dr. Harold A. Titus of the United States Naval Postgrad- 
uate School for his patient assistance in this work as thesis super- 
visor. 



ii 



TABLE OF CONTENTS 



Page 

Title Page i 

Abstract ii 

Table of Contents iii 

List of Illustrations iv 

Section 

1. Introduction 1 

2. Uncontrolled System 3 

3. Control with an Analog Computer 16 

4. Simulation by Graphs 22 

5. Optimal Discrete-time Control 26 

Appendix 

I. Graphs for No Control of Cart Position 35 

II. Graphs for Control of Cart Position 42 

III. Graphs for Discrete-time Control 48 

IV. FORTRAN Programs 53 



iii 



LIST OF ILLUSTRATIONS 



Figure Page 

1- 1 General Schematic 1 

2- 1-1 System To Be Stabilized 3 

2-1-2 Force Diagram Showing Equation of Motion 4 

2- 3-1 State Variables in Jordan Canonical Form 14 

3- 2-1 Cart Being Controlled by Analog Computer J.9 

3-2- 1-1 Controller Circuitry 20 

3-2-2- 1 Additional Network for Position Control 21 

1-1 Stick Angle Vs. Time without Position Control 36 

1-2 Stick Angle Changing Rate Vs. Time without 37 

Position Control 

1-3 Position Vs. Time without Position Control 38 

1-4 Phase Plane without Position Control 39 

1-5 Position Changing Rate Va Position without 40 

Position Control 

I- 6 Control Force Vs. Time without Position Control 41 

II - 1 Stick Angle Vs. Time with Less Position dontwl 43 

II - 2 Phase Plane with Less Position Control (b=0. 1) 44 

II- 3 Position Vs. Time with Less Position Control 45 

(b=0. 1) 

II-4 Stick Angle Vs. Time with More Position 46 

Control (b = 0. 7) 



IV 



Figure Page 

II- 5 Position Vs. Time with More Position Control 47 

(b=0. 7) 

III- 1 Stick Angle Vs. Time for Discrete-time Control 49 

III-2 Phase Plane for Discrete-time Control 50 

III- 3 Control Force Vs. Time for Discrete-time 51 

Control 

III- 4 Position Vs. Time for Discrete-time Control 52 



v 



1. Introduction 



It seems to be interesting to provide artificial stability for an 
inherently unstable physical system. Some immediate questions 
arise, such as: wljat are the largest errors which can possibly be 
corrected with a limited available control force, and what is the 
best control strategy which minimizes the time required and maxi- 
mizes the size of the system errors which can be corrected. 

In this simple case here, an inverted pendulum pivoted on top 
of a cart is to be stabilized by applying limited force to the cart 
through an electric motor. This inherently unstable system is 
assumed to be adequately represented by a set of linear differen- 
tial equations over the range of interest. The type of instability 
is represented by real non-negative characteristic roots. The 
motor supply voltage is manipulated, within its allowable limits, 
as a function of the state variables of the set of linear differential 
equations. 



In Chapter 3, an analog computer is used to really balance 
the pendulum. The general schematic of the system is shown here. 
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FIG. (1-1). General Schematic 
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In Chapter 4, the whole system is simulated with CDC 1604 
digital computer. Graphs are plotted and situations discussed. 
In Chapter 5, the technique, r of optimal discrete -time control 
is introduced. The results simulated by CDC 1604 turned out to 
be successful. 
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2. Uncontrolled System 

2. 1 Linear Differential Equations 

As shown in Fig.(2-l-l),the instantaneous angle that the pendu- 
lum makes with its unstable equilibrium position is 0, and the 
position of the cart with respect to some reference point on the 
floor is?... The coordinates are shown with positive displacement. 




FIG. (2-1-1). System To Be Stabilized 
For establishing the equations of motion, we define some addi- 
tional symbols: 
f force applied to cart 

f^ damping coefficient including friction and back e. m. f. 
fv applied voltage force coefficient 
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g acceleration of gravity 
M total system effective mass 
m mass of the pendulum 

r distance of pendulum mass center from hinge line 
T pendulum radius of gyration about hinge line 
V vjpltage applied to d. c. drive motor 

The equations of motion can be found by consideration of the 




FIG. (2-1-2). Force Diagram Showing Equations of Motion 
* Note: 0 is very small. 

The liniarized equations of motion are 

my 9 = mrg0 - mrje (2. 1. 1) 
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and 



M ~ mr0 + f (2.1.2) 

where 

f = f| i + f v V ( 2 . 1 . 3) 

We have data measurements as follows: 
g = 9. 8 / se c 2 

m = 0. 225 kg. 

M = 6 . 0 kg. i 

r = 0. 44 m. 



1 = length of longer part from hinge = 0. 94 m. 
h = length of shorter part from hinge = 0. 035 m. 

^ = (l 2 + 3 h 2 - 31h) x ^3 = 0. 25 m 2 
V = 24 volts (for selected D. C. motor) 

The force exerted on the cart is 13. 6 newton while the cart is 
held still, i. e. , =0, thus from (2. 1. 3) 



f = X = Il h in ; 57 n/-. 



V 



24 v 



In (2. 1. 2), we have mass of the pendulum much less than mass of 
thfe whole system, the term "mrG" can be neglected, given: 



M % = f = f| i + f v V (2. 1. 4) 

• • 

Securing the pendulum (9 = 0), we run the cart on the floor 
and observe the motion with a brush recorder, find the average 
velocity and acceleration as: 
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% = 0. 90 m / sec 



and 



% - 0. 83 m ^sec 



By (2. 1. 4) 

f , £li^Z = . n 8 n ‘ sec / m 
* 5 

Now (2. 1. 1) and (2. 1. 4) can be written as: 



a - it. 



f 1 



■fe . 






n 



■v 



Define: 

9-9 



9 = 9l = 02 

! = tl 

i*ii=h 






Then, we get a set of linear differential equations as: 



0, - 9 2 

i = ^2 

In matrix form: 
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9 2 
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,i 2 | 
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Vfv 



( 2 . 1 . 1 ) 

(2. 1. 4) 



(2. 1. 5) 
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Substituting numberical values: 



— 














r 
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01 




0 


1 


0 


0 




e i 
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' 9 2 





17. a 
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0 


3. 5 
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®2 
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-17. 2 


5i 
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0 
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1 




Si 




0 
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-2 

* 








9. 8 



Vfy 



< 2 . r. 5) 



Or, by defining some new matrix names, and x( s as the name of 
state variables: 



x = [A] x + £ 
where 

Vfv 
u " Mg 



u 



[4 - 

[A] - 



IX o 
T 2 



o 



0. 0 



0 0 
0 

1 






X = 



ooo 



% 

0 



2 

ii 











0 1 




0 


X - 


CD 

DO 


C = 


r 2 




Si 
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}* 




g 



( 2 . 1 . 6 ) 



(2. 1. 7) 



(2. 1. 8) 
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2. 2 Transformation to Canonical Form 



Assuming in our linear transformation matrix [Aj there is some 
eigenvector v associated with eigenvalue A ; 

[Al v = A v where V ^ 0 

[A - A I] v = 0 



or 



| A - A. 1 1 = 0 
By (2. 1. 8) 

-A. 1 

-A. 

0 

0 



11 

r 

0 



0 

0 

-A. 



Mf- 



0 



6 

n 



o m A 



= o 



There comes the characteristic equation: 

A(A 0 



The eigenvalues are: 

V- 



4. 15 



Az- + 7^ = + 415 

A 3 = 0 



A, 






/ 



n 

Only A^and A^are non-negative or unstable. 

With these eigenvalues all distinct, one can always find a new set 
of state variables: 
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yi 
y2 

^3 

y 4 

related to x by the transformation 

Z = [ c "r] 2£ 

such that the system of equations (2. 1. 6) transforms to: 



(2. 2. 1) 



/v. 






0 



0 



/V. 



7V, 



Z + D u 



( 2 . 2 . 2 ) 



with the 4x4 matrix [tqj and vector D given later. 



From (2. 2.1) 
i-l 



x -[erj 



(2. 2. 3) 



Let 



N 



-1 



g n 


g 12 


g 13 


§14 


§21 


g 22 


§23 


g 24 


g 31 


g 32 


g 33 


g 34 


g 41 


g 42 


g 43 


g 44 



The transformation matrix [£f] ^is not unique, but a convenient 
form for this problem can be found by four column vectors, all are 
eigenvectors associated with one eigenvalue respectively. 
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Define 



-1 



Where 
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2. 5) for i = 1 
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g ll' 
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0 - 
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g 21 
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- A 1 


1 




g 3l 
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0 


0 


-A+A, 




^41 



(2. 2. 4) 



(2. 2. 5) 



— 0 



( 2 . 1 . 8 ) 



There are many possible solutions, one set of which can be: 



g =0 

S 41 

g =0 

S 31 



g 



21 



g. 



AAz 

Az 



'll a 2 -a 1 

for i = 2, 3, 4 respectively (note A^ = 0), we can adopt: 



’42 



0 



g - 0 
s 32 



10 



g 



22 



g 



12 



X, Kj. 

Ai'Al 



Al 

Az'Ai 



g 43 ° 

g 33 = '®X 
g 23 " 0 
g l 3 = 0 



g 44 = S/;V 4 

g 34 = ^4 ( 

a = * 7 * 

6 24 (A 

g = zM 

(Ai"A4-)(a z -A^.) j 

Then 




Inverse of 




-x 2 

A 2 -Aj 

-Ai<A 2 

Az-A 2 
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0 

[°] 1 is 

-1 
-1 
0 
0 



Ar A j 

AiAz 

Az'Aj 
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0 

[O]: 
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Al 

.X 
a 2 

0 




( 2 . 2 . 6 ) 
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By (2. 1. 6) 



x = [ Aj x + c_ u 
By transformation 
2 - [g] X 

la] 1 i = [a][g]' 1 2 + cu 
i = £ + ( G] cu 



Define 



MM(c )- 1 £(j] - 



A. 



0 



0 



A 



A. 









Al 1 




d l 








d 




Ai. 


c ^ D 22 


2 

d 

3 
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i-'X 

2 




d 4 




; . -r 

> 

* 



Finally, we get the Jordan Canonical form of the system: 
I = (j]l + Du 



( 2 . 1 . 6 ) 



(2. 2. 7) 



( 2 . 2 . 8 ) 



(2. 2. 9) 



( 2 . 2 . 10 ) 
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2. 3 The Reduced System of Equations 



In the last section, the whole system is represented by equation 
(2. 2. 10). Now we have to show that for purpose of establishing a 
controller which assures stability of the equilibrium point at the origin, 
it is sufficient to consider the following reduced system. 



without regard for the coordinates and y^ , which associated 
with negative eigenvalues. 

If this is true, i. e. , a controller u = f (y2> y^) can be found 
which makes the system (2. 3. 1) asymptotically stable for some region 
about the origin of the two dimensional space. By definition: 
y^ — 0 as t — fx? i = 2, 3 

From (2. 3. 1) 

(y^ - d^u) — > 0 as t — *■ o& i = 2, 3 

Then, certainly, as t — > <xo 



h = yt + d i u 



i 



2,3 



(2. 3. 1) 




£+at 



l 



2, 3 



But 




Then 



f 



t+Af 



aw 




At o 
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This implies u (t) — s»-0 as t— -^ooin the sense of its average 
value over any very small non- zero time interval. 

Now, we come back to those equations for y^ and y . By 
means of Jordan canonical form, the state variables are express- 
ed by partial fraction as the following block diagram. 




FIG. (2-3-1). State Variables in Jordan Canonical Form 

Let In (t) be the impulse response for y., then 

(* ' 3 

Lim y. (ft) = Lim I h. (t-t ) u (t ) dt ■ = 1, 4 

t -»•*>© 3 ° J c 3 1 1 3 



Since hr (t) approaches zero exponentially as t && 

(because of negative eigenvalue), and since as mentioned above, 

u (t) can be considered to approach zero under any integral sign, 

it follows that y. — r 0 as t—r O o 
J 3 
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Thus the initial displacement of the states y and y^ have 
no effect on the region of stability of the complete system. There- 
fore, from now on, we only consider the reduced system described 
by equation (2s3sl). 
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3. Control with an Analog Computer 
3. 1 Selection of a Controller 

A controller is needed to provide stability in the region of 
controllability, which means the largest region in the state space 
within which the system can be brought to the point of equilibrium 
at the origin with the constraint u^.U. The controller will be a 
function only of yg and y^ , but through the transformation y = [G}x, 
it will generally involve all the state variables of the original 
system. Pontryagin 1 s maximum principle determines an optimum 
u (t) which minimizes: 

= 1 f ( y 2 > y 3 )dt 

4 

with the constraint 

|u| 

and final states 

Y i (1JT) = 0 i = 2, 3 

■£ is some positive cost function, different kinds of which 
lead to different kinds of controller. For the case of minimum- 
time controller = 1. We define a new state variable: 

y 4 - y„CT> - jja t 

It follows 

y = 1 
J 4 

By adding this new state variable, our system gets: 
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(3. 1. 1) 



/ 




f " 






y 2 




*2 y 2 




d 2 u 


y 3 


= 


0 


+ 


u 


1 




1 




0 

w i 1 



Pontryagin further defines the Hamiltonian function, maxi- 
mizing this H function with respect to u has the same effect as 
minimizing y ,at). 

In our case: 

|_| — JT = Y 2 P 3 LI + ?4- 



For maximization with respect to u, probably the bang- 
bang type controller (u = 1 U) is the best consideration. 

Let 



u = U sgn [ f J 

Usually f ' can be derived by solving the following canonical 
equations. 



y i = Ir 

P - - 

i zy. 

In this problem, it is hard to express them explicitly. In 
view of piecewise linear switching, the following guess seems 
reasonable. 



f = by 3 -y 2 

where b is some positive constant. 



(3. 1. 2) 



17 



Therefore 



u = U sgn (by 3 - y 2 J (3.1.3) 

If there is no control on cart position, that means is no longer 
a state variable. Then by equations (2. 2. 1) and (2. 2. 6) the 
uncoupled state y^ has no meaning. In this case the whole system 
(2. 3. 1) reduces to: 



y 2 = A 2 y 2 + d 2 u (3.1.4) 

u = U sgn [ -y 2 } (3. 1. 5) 

3. 2 Realization of the Control 

Now, we get a minimum-time controller, which is (through 
the functions of y 2 and y^) in terms of the original state variables, 
namely, 0^ y and ^ 2 . Those state variables can be gener- 

ated by two potentiometers and two techometers. We select a 
Donner Analog Computer to sum them up and use a D. C. relay to 
generate sgn function. The real structure is shown as Fig. (3-2-1). 
3. 2. 1 No Control of Cart Position 
By (3. 1. 5) 

u = U sgn (- y 2 ) (3. 1. 5) 

By (2. 2. 6) 



^2 



= _ 9 _ _L 0 + L^± 
e i A 2 °2 + 3 a.-a* 

= - 0 X - 0. 241 0 2 - 0. 138 






(3. 2. 1. 1) 
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Con-rolled by \naio; ( on.p- er 



I id. 



;l-2- ! ) . < r 



: U-’in; 




By measuring those potentiometer and tachometers, the 
following data are obtained. 

0. T 1 volt corresponds to 0. 087 radian 

m 

9. 1 volt corresponds to 40. 4 radian/ sec 

1 volt corresponds to 0. 36 meter/ sec 
Multiplying these factors, we get: 

u = U sgn [ 0.087 0j + 9. 75 0 2 + O. 05l 2 ] (3. 2. 1. 2) 

where 



U 



^J_ £y 



0. 24 



(3. 2. 1. 3) 



The circuitry is built for the controller as shown in Fig. (3-2-1-1). 




FIG, (3-2- 1-1). Controller Circuitry 
3. 2. 2 With Control of Cart Position 
By (3. 1. 3) 

u = U sgn [by 3 - y 2 ] (3. 1. 3) 
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From (2. 2. 6) 



y 3 = ~jjr£ l + -|-t 2 = 0 - 204 V 0102 (3. 2. 2,'l) 

For | . 

1 volt corresponds to 0. 05 meter 
Then 

y = 0.0102^+ 0.036? 

The following network is added to the connector in Fig. (3-2-1-lJ 




To Cowec-for 

of f/j (3-2-1- 1) 



FIG. (3-2-2- 1). Additional Network for Position Control 

If the value of b is not big enough, the cart has a tendency 
to settle itself a little bit off-center. For instance, taking b=0. 08 
(as shown in Fig. 3-2-2- 1), the cart tends to settle itself about 0. 2 
meter from its starting position. If the polarity of the potentio- 
meter for ^ is reversed, the cart will tend to settle in the opposite 
side from the starting position. Anyhow, if b is big enough, the 
cart will come back to its starting position. This is shown in 
appendix II- 5, for b = 0. 7. 



21 



4. Simulation by Graphs 

4. 1 The Region of Controllability 



When we are playing with the physical cart, we can start the 
cart motion by just pushing the pendulum off its vertical position. 
Now, for the case of simulation with digital computer, we encounter 
the problem of deciding the initial condition of 0^. In other words, 
we want to know the region of controllability which means the 
largest region in the state space from which the system still can 
be brought back to its equilibrium point. 

With a bang-bang type controller (u = t U), the trajectories 
consist of two segments corresponding to u = U and u = -U 
respectively. The origin of the two-dimensional space can always 
be reached by this switching of u. The region of controllability, 
then, can be defined as the set of points reachable by the trajec- 
tory starting at the origin (yg = 0, y3 = 0), and proceeding in 
reverse time((0^t co) with u alternately taking values of +U 
and -U. In our problem: 




(2. 3. 1) 



Those first order differential equations can be easily solved 



with solutions as: 
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y 2 + ( d 2 u /^ 2 ) 

y 20 + < d2U /^2^ 



exp [X 2 (t- to) J 



(4. 1. 1) 



y 3 ~ y 3Q 

d 3 u 




(4. 1. 2) 



From (4. 1. 2), y^ is undefinted as t— >-oo, no matter what the 
initial value y is. Thus the region of controllability is unbounded 
in the yg coordinate. Equation (4. 1. 1) shows directly that 

as t — y - CO , hence must be bounded by: 

N<f 2 




Numerically we have 




.0. 45 



(4. 1. 3) 



By (2. 2. 6) 

y 



= -9--k 9 4^ 



^2 



3 A 2- 



vV, 



If only 0 has non- zero initial value, it must be bound as: 



0 (0) 



0. 45 



This is the reason of using 0. 1 (radian) as the initial value of 
angle displacement. 

4. 2 Analysis by Graphs 

4. 2. 1 No control on Cart Position, Equation (2. 1. 5) can be 
written as: 
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0 



1 



0 



0 



X 1 

*2 

X 3 
x . 



17. 2 0 

0 0 

0 0 



0 3. 5 

0 1 
0 





+ s 




* s 




X 1 




0 




X 9 




-17. 2 


X 


Z 


+ 






x 3 




0 




X, 




9. 8 


> 


4 > 




* 



X u (2. 1. 5) 



By (3. 1. 5) and (3. 2. 1. 1): 

u = 0. 24 sgn f Xj + 0. 241 X2 + 0. 138 J (4. 2. 1. 1) 

Now, this system can be simulated. For better accuracy, we 
use sin x^ instead of x^. Graphs are shown in Appendix I. Same 
initial angle for all graphs. 

Fig. 1-1 shows pendulum angle vs. time, with initial angle 
displacement 0. 1 radian. It comes back very quickly. 

Fig. 1-2 shows the pendulum changing rate vs. time. 

Fig. 1-3 shows the cart position vs. time. 

m 

Fig. 1-4 shows the phase plane (0 vs. 0). The tracjectory 

does come back to the origin. It means stability. 

# 

Fig. 1-5 shows 2 vs. 2 , . As time goes on, the position 
rate decreases linearly to zero. 

Fig. 1-6 shows the control force vs. time. It is the bang- 
bang type force; only the direction of the force is switched by the 
relay. 



4. 2. 2 With Control on Cart Position. 

In this case, the system equations are same as before, but 
the control force changes as: 
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u = 0. 24 Sgn[x 1 + 0. 241x 2 +(0. 102b+0. 138)x 4 +0. 204bx 3 ] 

We simulate this system with the initial angle displacement 
as before (0 (o) = 0. 1 radian). Graphs are listed in Appendix II. 

Firstly with b = 0. 1 for position control. 

Fig. II- 1 shows pendulum angle vs. time. It is stable. 

Fig. II- 2 shows the phase plane. The tracjectory goes to the 
origin. 

Fig. II- 3 shows the cart position vs. time. It does not come 
back very quickly, because the amount of b is too small. 

Then take b = 0. 7 for position control. 

Fig. II- 4 shows the stick angle vs. time. 

Fig, II- 5 shows the position vs. time. The cart comes back 
to where it started very quickly. 
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5. Optimal Discrete -time Control 



5. 1 The System in Discrete-time Form 



Recall the original system as follows: 
x = (aJx + cu 



( 2 . 1 . 6 ) 



Consider, firstly, the equation without control force. 



x = [A] x 



(5. 1. 1) 



and assume a Taylor series 



x (t) = A, + A x t + A 2 t 2 +. . . +A m t m +. . . 



(5. 1. 2) 



to be the solution of the above homogeneous differential equation. 

Then, set t = 0, one obtains: 

x (0) = A 
— o 

Next, if (5, 1. 2) differentiated and then t set to zero, one obtains: 
x (0) = A 1 
But, from (5. 1. 1) 
x (0) = [a] x(0) 

Then 

A x =(A) x (0) 

If (5. 1. 2) is differentiated twice, and t set to zero, one 
obtaines: 

x (0) = 2A 2 



or 



2A 2 = x (0) = (a) x (0) = [A] 2 x (0) 
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So that 



a 2 = 1/ 2 [a] 2 5<0) 

Continuing this process, all the terms can be evaluated, 
and one obtains: 

x (t) = |(l] +f a] t + ^ t t2 ‘ + ^ m t^ +• • • j x (0) (5.1.3) 

By comparing it with the scalar expansion of e a ^, it is obvious 
to have a more compact form, like: 

x (t) = e At x (0) (5. 1. 3. 1) 

In imy case, e A ^ is a 4 x 4 matrix. It is usually called funda- 
mental matrix and designated by: 

<j) (t) Pe At 
Apparently 

^<- t)= e' At= -fV=-f 1( *> 

Also 

x (t ) = j> (t) x (o) 

Then 

x (t) = '( (t) x (o) = [A] x (t) = [a] f (t) x (o) 

So that 

0 (t) = (A) /f (t) (5.1.5) 

In order to solve the equation (2. 1. 6), we try to find a particu- 
lar integral in the form of 



2 ? 



X (t) = (t) y (t) 

By putting into (2. 1. 6 ), one obtains 

'f (t) y (t) + ^ (t) y (t) = [A] (t) y (t) + c u (t) 

By (5. 1. 5) 

^ (t) y (t) = c u(t) 

y (t) = f f\r)c u(Z)dT 

Jo 

Then 

x (t) = / (t) /V *&) ± Z 

* 0 

By (5. 1. 4) 

X p (t) = j (t )f o J(-T)Q U(r)Jiz (5. 1. 6) 

In evaluation of j) (t), we know the argument t represents the 
time interval between two instants. More conveniently, we can 
describe by: 

x (t 2 ) = 0 (t 2 - tj) X (t-j^) 

x (t 3 ) - f (t 3 - t 2 ) x (t 2 ) = f (t 3 - t j) x (ti) 

But *(■(;) = X(t L ) 

3^1') = / (*3" t 2>‘ I (t 2" *1* 

By this reason, (5. 1 . 6 ) can be put into the more familiar form of 
a convolution integral. 

x (t) = /*/ (t -?) c u (?) d? (5.1.6. 1) 

p 4 r 

Then the general solution of (2. 1. 6 ) will be: 



2B 



(5. 1. 7) 



X (t) = j b (t) X (o) + / t j (t-T) c u (z ) d T 

'o 

In case of discrete time, it turns out to be: 

x (k+1) = f (DT) x (k) + f BT f (DT -Z) c u (2T ) dZT (5. 1. 8) 

'0 

Where 

DT P sampling time 

By noting of a constant control force through the interval DT, 
one obtains: 

DTfllDT-ricdr (5.1.9) 

Or 

x (k + 1) = JZ( (DT)- x (k) + A u (k) (5. 1. 10) 

Where 

p fDT , 

£ ~ J f (DT -T) c dr 

° r i 2 2 r i m m 

6 (DT) = f I~1 +fA] • DT + l A) (DT) + . . . + [Aj (DT) + . . . 

7 2 1 ml 

The computation of £ (DT) and ^ (DT) would be very tedious, 

but, with the high speed computer, they can be obtained within 
seconds. The program to achieve this is shown in Appendix IV- 3. 
(DT = 0. 1 sec. ) 

Where 

^ -0. 0817 
-1. 6067 

A = 

0. 0458 
0. 8882 
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x (k + 1) = 



f (DT) x (k) + u(k)’ j 




1. 0872 


0. 1029 


0. 0000 


0. 0166 


1. 7697 


1. 0872 


0. 0000 


0. 3268 


0. 0000 


0. 0000 


1. 0000 


0. 0906 


0. 0000 


0. 0000 


0. 0000 


0. 8187 



5. 2 Evaluation of Control Force , 

In optimal discrete-time control, if we want to minimize 
the following cost function. 

J(n) =£. fx* (k) Q x (k) + ru 2 (k- 1 )1 (5. 2. 1) 

K=J l 

The second term represents the amount of control force 
which can be allowed, arbitrarily r set to 1. The first term 
gives the choice of state variables which will be minimized. In 
my case, x^ and Xg are those variables. So, Q becomes: 






0 



By (5. 1. 10) (5. 2. 1) 

J( n) =| ^x(n- 1)+Au(n- l)j^ Q |j/fx(n- 1) +A u(n- l)j +ru 2 (n- 1) + J(il- 1 ) 

Minimizing with respect to u(n-l), ' = 0 and noticing 

£ U (fl-l) 

Ji^n-1) is independent of u(n-l), gives 

|x t (n-1) ^+u(n- l)A t |QA+A t Q^ x (n-1) +Au(n- l)j+2ru(n- 1) = 0 



u(n-l) = -■“£ 






£ l QA+ r — 



x (n-1) 



(5. 2. 2) 
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Define 



t D_ Jof 

1 aI DA- 



A 1 QA+r 



(5. 2. 3) 



Then 



u(n- 1) = x (n- 1) 



(5. 2. 2. 1) 



Substitute u(n-l) to (5. 2. 1), one obtains: 



(5. 2. 4) 




+ r[ ]* aja^ [ J +[ ] t Q[ J+ ru 2 (n-2) + J(n-2) 



The first and third terms of (5. 2. 4) can be combined as: 



{[ + r hajA *} q {?[ J+Aaj'f )} + C ]* Q[ ) 

= C ] t (p t Qp + ^QAa^ a 1 A i Q / 0 + a 1 A t Q^a 1 t )[ ]+CJ t QCJ 

= C 3 t ( f +4a/}+ a 1 A t Q<^ +Aa 1 t >)[ J + [ Q[ ] 

= [ ] t (0 t + a^) Q (fi +4a 1 t )( J +[ ) t Q[) 

= C 3 Qf t )C 3 + C I'qC ] 

= C 3 t (fi t Qix + Q)C ] 



J(n) = L ] t ('i/'i t QV'' 1 +Q)[ J + r £ ] t a 1 a 1 t [ J + ru 2 (n-2) + J(n-2) 
= [ ] t Qy' 1 +Q + ra 1 a| : )[ } + ru 2 (n-2) + J(n-2) 



Where 



a x = (a 1 ' t ) t 

[ x (n-2) +Au (n-2)] 



Further defining 




(5, 2. 5) 



Now, (5. 2. 4) becomes: 
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Define 



Pi ? 'fl Q^+Q + ra^J 
= 0 gives: 

u(n- 2 ) 

( ] l p^ +A t p i [ ] + 2 ru(n-2) = 0 

fx 1 - (n- 2 ) + u (n-2)A t lp A + 2 ^ L ] - 

t 11 

u (n- 2 ) = — — 1 * ^ — x (n- 2 ) 

A%A-tr ~ 

Define 

t = _ A* Pi j_ 
a 2 A t p ] A+ r 

Then 

u(n- 2 ) = ^2 x (n- 2 ) 

Define p t? Q 

o “ 

(5. 2. 3) becomes: 

t . . SpJ_ 

1 A 1 p A +r 

Continuing one more stage, and set - 

^u(n-3) 

^2 = t +Aa 2 l 

P2 = V / 2 t P 1 V2 + Q + ^ a 3 a 2 1 

t / 

d A t P 2 A+ r 

u(n-3) = a.^ x (n-3) 

Continuing on the same procedure, one 
following general forms. 



(5. 2. 6 ) 

2 ru (n- 2 ) = 0 

(5. 2. 7) 

(5. 2. 8 ) 

(5. 2. 7. 1 ) 

(5. 2. 3. 1 ) 
= 0 , one obtains: 



can expect the 
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A i* +ZSa k‘ 

Pk = ^k t Pk-iA + Q + ra k a k t 



, 4- p k,.i/ . 

ak ' A p k-l &+r 

u(n-k) = a t x (n-k) 
k ~ 

We note that u(n-k) depends solely on those present states 
x (n-k). Thus makes clear Bellman's ^Principle of Optimality", 
which states: "An optimal policy has the property that whatever 
the initial state and the initial control input rector are, the 
remaining control input rectors must constitute an optimal policy 
with regard to the state resulting from the first control signal. " 

When the number of stages gets very large, a, * converges 

k 

to some final set of values. Thus exists some fixed values for 
the feedback compensation for all values of time and state variables. 

With the help of CDC 1604 computer, 200 stages are accom- 
plished. The program is shown in Appendix IV-4. Finally, one 
obtains, for all sampling time: 



2. 4846' 



u(t) = 



0. 5990 
0. 0074 



x (t) 



0. 3448 



(5. 2. 9) 
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5. 3 Simulation by Integration 



With the control force shown in (5. 2. 9), we can simulate 
the system by calling a subcontine for integration. The control 
force is calculated during every sampling instant and hold con- 
stant until the next sampling instant. 

The FORTRAN program achieving this purpose is shown 
in Appendix IV- 5. Initial angle displacement is same as before, 
x (1) = 0. 1 radian. Sampling time DT = 0. 1 second. 

The graphs are collected in Appendix III. 

Fig. Ill - 1 shows stick angle vs. time. The angle comes 
back after about 40 stages. 

Fig. Ill - 2 shows phase plane. 

Fig. Ill- 3 shows control force vs. time. 

Fig. Ill - 4 shows cart position vs. time. It gets back to the 
starting position comparatively slowly. 



34 



APPENDIX I 



Graphs for No Control of Cart Position 

1. Stick Angle vs. Time Without Position Control 

2. Stick Angle Changing Rate vs. Time Without Position Control 

3. Postion vs. Time Without Position Control 

4. Phase Plane Without Position Control 

5. Position Changing Rate vs. Position Without Position Control 

6. Control Force vs. Time Without Position Control 
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• Jja0E-4ft0 UTUT5/JMCM V. 

'I-.SMLC « VMt-HQ LUJTVJrm 

PIRN MO POSITION CONTROL 
RUN I STICK RNGLE US T 

X-Scale: 1. 0 Second/Unit 

Y-Scale: 0. 02 Radian/Unit 

FIG. (1-1). Stick Angle Vs. Time Without Position Control 
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xomu * jje>«-»a0 uwTVjrm 
i -.5 ml it - jjaec-aj uruTS/JMca 

WAN MO POSJTJON COMTROL 
RUM 1 flMGLE RATE US T 

X Scale: 1. 0 Second/Unit 

Y Scale: 0, 1 Radian per Second/Unit 

FIG. - (1-2). Stick Angle Changing Rate Vs. Time Without Position Control 
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mft aaj aao mo 004 005 aee && 



X'SCflLt - j jaat+aa unjts/jnck 

'f'SCRIE * 5^-09 UNITS/ JNCtt 

WAN NO POSITION CONTROL 
RUN I POSITION US T 

X Scale: 1, 0 Second/Unit 

Y Scale: 0. 05 Meter/Unit 

FIG. (1-3). Position Vs. Time Without Position Control 
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XOMLIt - S.ftBE-00 UHJTVJNCH. 

'I-.SMLIE - UMJT5/JHCH. 

klAH HO P0SJTI0H COHTROL 
RUH J PHASE PLAME 

X Scale: 0. 02 Radian/Unit 
Y Scale: 0. 1 Radian per Second/Unit 

FIG. (1-4). Phase Plane Without Position Control 
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C 00 S00 300 we £00 *60 r 00 000 




x-smle - s.aae -&2 uruTVjrm 
- jjaae-aj uwts/Jhcm. 

l-JRN MO POSITION CONTROL 
RUN 0 ZDOT US Z 

X Scale: 0. 05 Meter/Unit 
Y Scale: 0. 1 Meter per Second/Unit 

FIG. (1-5). ‘ Positidn'Chdngihg FTate Vs. Position Without Position Control 
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X-SMLlt - j^uat-taa umT3/JMCh. 

'i-3CA.it - jji0E-aj DKiTVJntn 

WAN NO POSITION CONTROL 
RUN O U US T 

Note : Control force has no dimension. 

X Scale: 1. 0 Second/Unit 

Y Scale: 0. 1 Unit/Unit 

FIG. (1-6). Control Force Vs. Time Without Position Control 
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APPENDIX II 



Graphs for Control of Cart Position 

1. Stick Angle Vs. Time With Less Position Control (b = 0. 1) 

2. Phase Plane With Less Position Control (b = 0. 1) 

3. Position Vs. Time With Less Position Control (b = 0. 1) 

4. Stick Angle Vs. Time With More Position Control (b = 0. 7) 

5. Position Vs. Time With More Position Control (b = 0. 7) 
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/-.SCALE ■= j^aaE-taa mjTVJNCH. 

-i-scale • ^.m-w uins/jrm 

UIAN WITH POSITION CONTROL 
RUN J STICK ANGLE US T 

X Scale: 1. 0 Second/Unit 
Y Scale: 0. 02 Radian/Unit 

FIG. (II-l). Stick Angle Vs. Time With Less Position Control (b=0. 1) 
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*-3MLIt * r 2,m.-£fi UWT5/JMCH. 

'1‘StfiUL - J.a6E-aj UWT3/JMCh. 

klflN UIJTH POSITION CONTROL 
RUN 1 PHASE PLANE 

X Scale: 0. 02 Radian/Unit 
Y Scale: 0. 1 Radian per Second/Unit 

FIG. (II- 2 ). Phase Plane With Less Position Control (b=0. 1) 
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.x-smlk = i.aat-»j0e> unitvinch. 

^ -.5 ML* - s.&aE-ao units/Incm. 

UlflN WITH POSITION CONTROL 
RUN I POSITION US T 

X Scale: 1. 0 Second/Unit 

Y Scale: 0. 05 Meter/Unit 

FIG. (II- 3). Position Vs. Time With Less Position Control (b = 0. 1) 
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X-SMlfi « UNITS/ INCH 

'i-SMU • r i.m.-zn uniTS/itm 

WAN WITH POSITION CONTROL 
RUN 0 STICK ANGLE US T 

X Scale: 1. 0 Second/Unit 

Y Scale: 0. 02 Radian/Unit 

FIG. (II -4). Stick Angle Vs. Time With More Position Control (b^O.'J) 
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*-5CfllK - j UKJTVJhCM 
't-vSMUC - h„m.'&2 UHJT5/'JhCM 

WAN UI1TH P0SIT10M CONTROL 
RUM Q POSITION US T 

X Scale: 1. 0 Second/Unit 

Y Scale: 0. 05 Meter/Unit 

FIG. (II- 5). Position Vs. Time With More Position Control (b = 0. 7) 
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APPENDIX III 



Graphs for Discrete-time Control 

1. Stick Angle Vs. Time for Discrete-time Control 

2. Phase Plane for Discrete-time Control 

3. Control Force Vs. Time for Discrete-time Control 

4. Position Vs. Time for Discrete-time Control 
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x-scpul - units/inch. 

'l-SCfilt » 'IJtft-en UhUT5/JhCM 

klflN DISCRETE TIME CONTROL 
RUN 1 STICK ANGLE US T 

X Scale: 1. 0 Second/Unit 
Y Scale: 0. 02 Radian/Unit 

FIG. (III-l). Stick Angle Vs. Time for Discrete-time Control 
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X-3MLK * WX* -en UMJ TS' JMCM 
'f-SMLIE « S.m-£Q UHJT5/JMCM 

klfiM DISCRETE TIME COMTROL 
RUM J PURSE PL RIME 

X Scale: 0. 02 Radian/Unit 
Y Sckle: 0. 05 Radian per Second/Unit 

FIG. (Ill - 2 ). Phase Plane for Discrete-time Control 
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flUL - UNJTS/JNCK 

'1-StfiUl - SMt-ZQ. UhJTVJMCM 

UlflN DISCRETE TIME COMTROL 
RUN 1 U US T 

Note: Control force has no dimension. 

X Scale: 1. 0 Second/Unit 

Y Scale: 0. 05 Unit/Unit 

FIG. (Ill- 3). Control Force Vs. Time for Discrete-time Control 
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K-scfiLfL •» ojaec-*aj unjtvjnch. 

't-5C^L(E - SJML-en UNITS'JNCH. 

LIRN DISCRETE TINE CONTROL 
RUN 0 POSITION US T 

X Scale: 20. 0 Second/Unit 
Y Scale: 0. 05 Meter/Unit 

FIG. (III-4). Position Vs. Time for Discrete-time Control 
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APPENDIX IV 



FORTRAN Programs 

1. FORTRAN Program for Simulation without Position Control 

2. FORTRAN Program for Simulation with Position Control 

3. FORTRAN Program for Calculating (j) and A Matrix 

4. FORTRAN Program for Calculating Discrete-time Control Force 

5. FORTRAN Program for Simulation of Discrete-time Control 
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.. JOB0250F*WAN 

PROGRAM BROOM 

C NO CART POSITION CONTROL 

DIMENSION XOO) *XD0T(30) .C(15) ' 

C< 10)=1.0 

1 CALL INTEG1 (T*X*XDOT*C) 

XDOT ( 1 ) *X ( 2 ) 

XDOT(2)*17.2*SINF(X( 1) >+3*5*X(4)-17*2*U 
XDOT ( 3 ) C X ( 4 ) 

XDOT < 4 ) =— 2* 0*X ( 4) +9* 8*U 

U=0.24*SIGNFU*»X<1)+C(1)*X<2)+C(2)*X(4>) 

X(5)*U 

GO TO 1 

END 

END 
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PROGRAM BROOM 
POSITION CONTROL 

RUN NO. TWO FOR MORE Y3 FEED BACK 
DIMENSION X ( 3 0 ) > XDOT (30) * C ( 15) 

C ( 10 ) = 1 . 

1 CALL INTEG1 (T,X,XDOT»C) 

XDOT ( 1) =X(2) 

XDOT ( 2 ) =17. 2*SINF( X( 1) ) +3 . 5*X ( A ) -1 7‘. 2*U 
XDOTT 3 ) =X ( 4 ) 

XD0T(4) =-2.*X(4)+9.8*U 

U=0 . 24*S I GNF ( 1. *X( 1)+C(1)*X(2 ) + ( C ( 3 ) *0 . 102+C ( 2 ) ) 
1*X ( 4)+C( 3 ) *0.204*X ( 3 ) ) 

X( 5 )=U 
GO TO 1 
END 
END 
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.. JOB0250F,WAN 

PROGRAM PHIDEL 

COMPUTING PHI AND DELTA MATRIX 
DT = SAMPL ING TIME 

DIMENSION A(12»12)*PHI(12*12) » T E R M ( 12,12) »WORM( 12,12) 

1 , DELTA) »DELM( 4, A) ,TELM( 4, A ) ,DELP( 4, A ) ,C ( A ) 

N = 4 

dt=o:i 

T M = 0 • 0 

RE ADI, ( ( A ( IR, IC .) , IC=1 ,N) , IR=1,N) 

1 FORMAT ( ( 4F2 0 • 8 ) ) 

READ 1 , (C( I ) , 1 = 1 ,N) 

1003 PRINT 399, DT ,( (A( IR, IC) » I C = 1 , N ) » I R=1 ,N ) 

3 99 FORMAT ( ///IX , 3HDT = » 1 F5 . 3/ / / , IX , 7HA ( I , J ) =/ , < ( 4F1 0*2)) ) 
PRINT 3991 (C( I.) ,1=1, N) 

3991 FORMAT ( ///1X»5HC( I)=/(4Fl0.2)) 

C 

DO 400 IR=1,N 

DO 400 I C = 1 ,N 

TERMt I R , I C ) =0.0 

WORM( IR , IC) =0.0 

TERMt IR»IR)=1«0 

TELM ( I R , I C ) =TERM( IR, IC)*DT 

DELPt I R , IC ) =TELM( IR, IC) 

.DELMt I R , I C ) =0.0 
DEL ( I R ) =0 . 

400 PHI ( IR, IC)=TERM( IR, IC) 

C 

4 TM= 1 . 0+TM 

DO 5 00 I R = 1 , N 
DO 500 I C = 1 , N 

DO 500 JN= 1 »N 

DELMt I R , IC ) =DELM( IR» IC J-TELMl I R» JN )*A( JN, IC) *DT/ ( TM+1 .0 ) 
500 WORM ( I R , I C ) =TERM ( I R , JN ) *A ( JN , I C ) *DT/ TM+WORM ( IR, IC) 

DO 401 I R = 1 » N 
DO 401 I C = 1 , N 
TERMt I R , I C ) =WORM ( IR, IC) 

TELM ( I R , I C ) = DF.LM< IR, IC) 

DELPt I R , IC) =DELP( IR, IC)+TELM( IR,IC) 

PHI ( IR, IC) =PHI ( IR, IC )+TERM( IR , IC ) 

DELMt I R , I C ) =0 . 

401 WORM ( I R , I C ) =0.0 
ABC=0 . 0 

DO 2 I = 1 , N 
DO 2 J=1,N 
AA=TERM( I , J ) 

AB=ABSF(AA) 

I F ( ABC-AB ) 3,3,2 
C FIND BIGGEST VALUE 

3 ABC =AB 

2 CONTINUE 

IF ( 0.000000005-ABC ) 5,5,6 

5 GO TO 4 

6 PRINT 502 , ( ( PHI ( IR , IC ) , IC=1 ,N) , IR = 1 ,N) 



502 FORMAT ( ///1X»9HRHI (I,J)=/((4F15.9))> 



DO 600 1=1, N 
DO 6 00 K= 1 * N 
DO 600 J= 1 , M ' 

60 0 DEL { I ) =DEL ( I )+PHI ( I »J)#DELP(J»K)*C{K) 
PRINT 503 (DELI I ) , 1 = 1, N) 

503 FORMAT ( ///1X*7HDEL ( I)=//(4F15.9)) 

END , 

END 

DT = 0 # 1 0 



A ( I » J ) = 



.00 1.00 


.00 


.00 




17.20 .00 


.00 


3.50 




.00 .00 


.00 


1.00 




.00 .00 


.00 


-2.00 




C ( I ) = 








.00 -17.20 


.00 


9.80 




PHI ( I, J)= 








1.087239756 


0.102891421 


0.000000000 


0.016631936 


. '1.769732445 


1.087239756 


0.000000000 


0.326856101 


0.000000000 


0.000000000 


1.00000000 


.090634623 


0.000000000 


0.000000000 


o.oooooooo 


.818730753 


DE L ( I ) = 








-.081750092 


-1.606739468 


0.045890345 


0.888219310 
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. . JOB0250F ,WAN 

PROGRAM OPT CON 

C MINIMUM J=SUM ( XT ( K ) *Q*X ( K. ) +R*U**2 ) 

DIMENSION PHI (.4,4) , PSI (A, A) ,P( A, A) > DEL ( A ) >AT ( 20 » A ) » 
1 GM ( » A ) » Q ( A » A ) » FM ( A ) » EM ( A ) 

READ 1 * N > NM > NPR I NT 

1 FORMAT (8110) 

READ2 » R »DT 

READ2 » ( (Q( I > J ) » J= 1 » N ) » I = 1 »N ) 

READ2 » ( (PHI ( I » J > , J=1 ,N> » 1 = 1 »N> 

READ2 » ( DEL ( I ) , 1 = 1, N) 

2 FORMAT ( (AF16.9) ) 

PRINT 3»N»NM»N PRINT 

3 FORMAT ( /// ( 8 I 10 ) ) 

PRINT AA,R,DT 

AA FORMAT ( //1X»2HR=»F5»2»3X» 3HDT = » F5 • 2 ) 

PRINT A5>( (Q( I»J) » J= 1 » N ) » I = 1 » N ) 

A 5 FORMAT ( //IX »7HQ( I * J ) =/ ( ( AF16 .9 ) ) ) 

PRINT A6 » ( (PHI ( I ,J) »J = 1»N) , I = 1»N) 

A6 FORMAT (//1X»9HPHI ( I »J)=/( ( AF16 • 9 ) ) ) 

PRINT A7» ( DEL ( I ) , 1=1, N) 

A 7 FORMAT (// IX, 7HDEL( I ) =/ ( AF 1 6 . 9 ) ) 

DO 5 I = 1 > N 
DO 5 J= 1 > N 
GM ( I ,J)=0.0 
EM ( I ) =0 .0 
FM ( I ) =0 .0 
P( I » J)=Q( I » J) 

5 PSI ( I » J ) = 0 . 0 

CALCULATE AT(K»J) 

DO 22 KK= 1 » NPR I NT 
DO 20 K= 1 > NM 
DEN = 0.-0 
DO 6 1=1, N 
DO 6 J = 1 » N 

6 EM( I )=EM( I )+DEL(J)*P(J»I ) 

DO 8 I = 1 , N 
DO 7 J = 1 » N 

7 FM( I )=FM( I )+EM( J)*PHI ( J, I ) 

8 DEN=DEN+EM ( I )*DEL( I ) 

DEN=-DEN-R 
DO 10 1=1, N 
AT ( K , I ) =FM ( I ) /DEN 
FM ( I ) =0 .0 

10 EM ( I )=0.0 

CALCULATE PSI(K,I,J) 

DO 13 1=1, N 
DO 13 J = 1 »N 

13 PSI ( I » J)=PHI ( I , J)+DEL( I )*AT(K, J) 

CALCULATE P(K,I,J) 

DO 16 1=1, N 
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DO 16 J = 1,N 
DO 15 L = 1 »N 
DO 1.5 M = 1»N 

15 GM( I »J ) =GM ( I , J ) +PSI ( L > I ) *P ( L»M) *PSI (M, J ) 
P( I , J) =GM( I ♦ J) +R*AT ( K» I )#AT( K» J ) 

C FOR TERMINAL CONTROL OMIT Q(I.J) 

16 GM( I '» J) =0.0 
20 CONTINUE 

PRINT 

PRINT 12»KK» (AT(NM.J) . J=1.N) 

12 FORMAT ( //IX »3HAT (»I2»2H)=/(4F16.9)) 

22 CONTINUE 
END 
END 



AT ( 10) = 

2.48A60AA17 .5990829066 .007459609 



.344834834 
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. . JOB0250F ,WAN 

PROGRAM BROOM 

OPTIMAL DISCRETE TIMF CONTROL 

FOR SIMPLICITY. USE AT ( J ) IN LAST STAGE FOR ALL U 
DIMENSION X { 30 ) , XDOT (30) >C(15) 

C ( 1 0 ) = 1 . 

1 CALL INTEG1 (T,X,XDOT,C) 

XDOT\( 1 ) =X ( 2 ) 

XDOT ( 2 ) =17.2*SINF( XI 1 ) ) +3 . 5*X ( 4 ) -17 . 2*U 
XDOT ( 3 ) =X ( 4 ) 

XDOT (4) =-2.*X(4)+9.8*U 
DT = 0 . 1 
C DT = HOLD TIME 

IF ( T-C ( 5 ) ) 1,2,2 

C ABOVE STATEMENT AS A ZERO ORDER HOLD 

2 U=C(1)*X( 1)+C(2)*X(2)+C(3)*X(3)+C(4)*X(4) 

X ( 5 ) =U 

C ( 5 ) =C ( 5 ) +DT 
GO TO 1 
END 
END 
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Control of a simple case of inherently u 
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